
#include <MPU6050_tockn.h>
#include <Wire.h>
#include <SPI.h>
#include <nRF24L01.h>
#include <RF24.h>

int z;

MPU6050 mpu6050(Wire);
RF24 radio(7,8);
const byte address[6] = "00001";
//小车部分
const char xc[]="n_xc";

const char go[] ="a_go";
const char back[] ="b_back";
const char left[] ="c_left";
const char right[] ="d_right";
const char left_turn[] ="e_left_turn";
const char right_turn[] ="f_right_turn";
//机械臂部分
const char jxb[]="g_jxb";

const char forward[]="h_forward";
const char baskword[]="i_baskword";
const char leftword[]="j_leftword";
const char rightword[]="k_rightword";

const char nip[]="l_nip";
const char lay[]="m_lay";
const char stop[]="0_stop";

//const char lazy[]="p_lazy"

void setup() {
  pinMode(2,INPUT_PULLUP);
  pinMode(4,INPUT);
  pinMode(5,INPUT_PULLUP);
  pinMode(9,INPUT_PULLUP);
  Serial.begin(9600);
  Wire.begin();
  mpu6050.begin();
  mpu6050.calcGyroOffsets(true);
  radio.begin();
  radio.openWritingPipe(address);
  radio.setPALevel(RF24_PA_MIN);
  radio.stopListening();
}
//radio.write(&text, sizeof(text));

//  Serial.print(mpu6050.getAngleX());
//  Serial.print("\tangleY : ");
//  Serial.print(mpu6050.getAngleY());
//  Serial.print("\tangleZ : ");
//  Serial.println(mpu6050.getAngleZ()-1);

//if(digitalRead(9)==LOW)
//  {
 //   delay(100);
 //    if(digitalRead(9)==LOW)
 //    {
 //     analogWrite(A3,255);
 //    }
 // }
void loop() {
  mpu6050.update();
  if(digitalRead(4)==1)
  {
    Serial.print(nip);
    radio.write(&nip, sizeof(nip));
    delay(100);
  }
  else if(digitalRead(2)==LOW)
  {
    delay(100);
    if(digitalRead(2)==LOW)
     {
      Serial.print(lay);
      radio.write(&lay, sizeof(lay));
      delay(100);
     }
  }
if(mpu6050.getAngleX()>=50 && mpu6050.getAngleX()<=90)
{
  Serial.print(mpu6050.getAngleX());
  radio.write(&left, sizeof(left));
  delay(100);
}
else if(mpu6050.getAngleX()<=-50 && mpu6050.getAngleX()>=-90)
{
  Serial.print(mpu6050.getAngleX());
  radio.write(&right, sizeof(right));
  delay(100);
}
else if(mpu6050.getAngleY()>=50 && mpu6050.getAngleY()<=90)
{
  Serial.print(mpu6050.getAngleY());
  radio.write(&go, sizeof(go));
  delay(100);
}
else if(mpu6050.getAngleY()<=-50 && mpu6050.getAngleY()>=-90)
{
  Serial.print(mpu6050.getAngleY());
  radio.write(&back, sizeof(back));
  delay(100);
}
else if((mpu6050.getAngleZ()-1)>=50  && (mpu6050.getAngleZ()-1)<=90)
{
  Serial.print((mpu6050.getAngleZ()-1));
  Serial.print("\tangleZ : ");
  radio.write(&left_turn, sizeof(left_turn));
  delay(100);
}
else if((mpu6050.getAngleZ()-1)<=-50 && (mpu6050.getAngleZ()-1)>=-90)
{
  Serial.print(mpu6050.getAngleZ()-1);
  Serial.print("\tangleZ : ");
  radio.write(&right_turn, sizeof(right_turn));
  delay(100);
}
else
{
  Serial.print("stop");
  radio.write(&stop, sizeof(stop));
  delay(100);
}
}

